#include <WiFi.h>
#include <WebServer.h>
#include<Arduino.h>
#include "Esp32McpwmMotor.h"
#include "Esp32PcntEncoder.h"
#include "PIDController.h"
#include "Kinematics.h"


Esp32McpwmMotor motor;   //创建一个名字为motor的对象
Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
PidController pid_controller[2];
Kinematics kinematics;

float target_linear_speed = 0.0; // mm/s
float target_angular_speed = 0.0; // rad/s

float out_left_speed = 0.0;
float out_rigt_speed = 0.0;

// 热点信息
const char* ssid = "FishBot_AP";
const char* password = "12345678";

// 创建Web服务器
WebServer server(80);

// 模拟电机控制函数
// void moveForward() { Serial.println("Forward"); }
// void moveBackward() { Serial.println("Backward"); }
// void turnLeft() { Serial.println("Left"); }
// void turnRight() { Serial.println("Right"); }
// void stopMotor() { Serial.println("Stop"); }
void updateSpeeds() {
  kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_rigt_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_rigt_speed);
}
void moveForward()
{
  target_linear_speed = 200.0;
    target_angular_speed=0.0;
    updateSpeeds();
}
void moveBackward()
{
    target_linear_speed = -200.0;
 target_angular_speed=0.0;
      updateSpeeds();
}
void turnLeft()
{
  target_linear_speed = 0; // mm/s
  target_angular_speed = 0.3; // rad/s
    updateSpeeds();
}
void turnRight()
{
  target_linear_speed = 0.0; // mm/s
  target_angular_speed = -0.3; // rad/s
    updateSpeeds();
}
void stopMotor()
{
  target_linear_speed = 0.0; // mm/s
  target_angular_speed = 0.0; // rad/s
  updateSpeeds();
}


// 网页内容
void handleRoot() {
  String html = R"rawliteral(
    <!DOCTYPE html>
    <html>
    <head>
      <meta charset="UTF-8" />
      <title>FishBot Control</title>
      <style>
        body {
          display: flex;
          flex-direction: column;
          align-items: center;
          justify-content: center;
          height: 100vh;
          font-family: Arial, sans-serif;
          background-color: #f0f0f0;
          margin: 0;
        }
        h1 {
          margin-bottom: 20px;
          color: #333;
        }
        .button-grid {
          display: grid;
          grid-template-columns: repeat(3, 120px);
          grid-gap: 15px;
        }
        button {
          padding: 20px;
          font-size: 18px;
          font-weight: bold;
          border: none;
          border-radius: 8px;
          background-color: #007BFF;
          color: white;
          cursor: pointer;
          transition: background-color 0.3s ease;
        }
        button:hover {
          background-color: #0056b3;
        }
        .speed-control {
          margin-top: 30px;
          display: flex;
          flex-direction: column;
          align-items: center;
        }
        input[type=range] {
          width: 300px;
        }
      </style>
    </head>
    <body>
      <h1>FishBot Control</h1>
      <div class="button-grid">
        <button onclick="setDirection(1)">Forward</button>
        <button onclick="stop()">Stop</button>
        <button onclick="setDirection(-1)">Backward</button>
        <button onclick="setTurn(0.3)">Left</button>
        <div></div>
        <button onclick="setTurn(-0.3)">Right</button>
      </div>

      <div class="speed-control">
        <label for="speedRange">Speed: <span id="speedValue">0</span> mm/s</label>
        <input type="range" id="speedRange" min="0" max="500" step="10" value="0" oninput="updateLinearSpeed(this.value)">
      </div>

      <script>
        let currentDir = 1;            // 1=前进，-1=后退
        let currentLinearSpeed = 0;    // 线速度
        let currentAngularSpeed = 0;   // 角速度

        function sendMove() {
          fetch('/move?x=' + (currentLinearSpeed * currentDir) + '&z=' + currentAngularSpeed);
        }

        function updateLinearSpeed(val) {
          currentLinearSpeed = parseInt(val);
          document.getElementById('speedValue').innerText = currentLinearSpeed;
          sendMove();
        }

        function setDirection(dir) {
          currentDir = dir;
          sendMove();
        }

        function setTurn(z) {
          currentAngularSpeed = z;
          sendMove();
        }

        function stop() {
          currentLinearSpeed = 0;
          currentAngularSpeed = 0;
          document.getElementById('speedRange').value = 0;
          document.getElementById('speedValue').innerText = 0;
          sendMove();
        }
      </script>
    </body>
    </html>
  )rawliteral";

  server.send(200, "text/html", html);
}





// 处理控制指令
void handleMove() {
  if (server.hasArg("x") && server.hasArg("z")) {
    target_linear_speed = server.arg("x").toFloat();  // 直接正负决定方向
    target_angular_speed = server.arg("z").toFloat();
    updateSpeeds();
    server.send(200, "text/plain", "OK");
  } else if (server.hasArg("dir")) {
    String dir = server.arg("dir");
    if (dir == "forward") moveForward();
    else if (dir == "backward") moveBackward();
    else if (dir == "left") turnLeft();
    else if (dir == "right") turnRight();
    else if (dir == "stop") stopMotor();
    server.send(200, "text/plain", "OK");
  } else {
    server.send(400, "text/plain", "Missing args");
  }
}

void setup() {
  Serial.begin(115200);


    // 2.设置编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接

  motor.attachMotor(0,22,23);//将电机0连接到引脚22和引脚23
  motor.attachMotor(1,12,13);//将电机1连接到引脚12和引脚13

  //pid_controller 初始化PID
  pid_controller[0].update_pid(0.625,0.125,0.0);
  pid_controller[1].update_pid(0.625,0.125,0.0);

  pid_controller[0].out_limit(-100,100);
  pid_controller[1].out_limit(-100,100);
  pid_controller[0].update_target(100); //使用mm为单位,为了提高精度
  pid_controller[1].update_target(100);

    //初始化运动学参数
  kinematics.set_wheel_distance(175); //mm
  kinematics.set_motor_param(0,0.105805);
  kinematics.set_motor_param(1,0.105805);

  //测试下运行学逆解
  kinematics.kinematics_inverse(target_linear_speed,target_angular_speed,&out_left_speed,&out_rigt_speed);

  // Serial.printf("OUT:out_left_speed = %f,out_rigt_speed = %f",out_left_speed,out_rigt_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_rigt_speed);

  WiFi.softAP(ssid, password);
  Serial.print("AP IP: ");
  Serial.println(WiFi.softAPIP());
  
  server.on("/", handleRoot);
  server.on("/move", handleMove);
  server.begin();
}

void loop() {
  server.handleClient();
  delay(10); // 等待10毫秒
  kinematics.update_motor_speed(millis(),encoders[0].getTicks(),encoders[1].getTicks());
  motor.updateMotorSpeed(0,pid_controller[0].update(kinematics.get_motor_speed(0)));
  motor.updateMotorSpeed(1,pid_controller[1].update(kinematics.get_motor_speed(1)));
  // Serial.printf("speed1 = %f,speed2 = %f\n",out_left_speed,out_rigt_speed);
}
